#ifndef _DYN_A_STAR_H_
#define _DYN_A_STAR_H_

#include <Eigen/Eigen>
#include <iostream>
#include <memory>
#include <plan_env/grid_map.h>
#include <queue>
#include <vector>
#include <chrono>

struct Node;
typedef std::shared_ptr<Node> NodePtr;

struct Node
{
  enum enum_state {
    OPENSET = 1,
    CLOSEDSET = 2,
    OCCUPIED = 3,
    UNDEFINED = 4
  };

  // Distinguish every call
  // 这样就不用每次都构建然后析构了
  int rounds{0};
  enum enum_state state {
    UNDEFINED
  };
  Eigen::Vector3i index;

  double gScore{0.0}, fScore{0.0};
  NodePtr parent{nullptr};
};

class AstarComparator
{
public:
  bool operator()(NodePtr node1, NodePtr node2)
  {
    return node1->fScore > node2->fScore; // 小顶堆
  }
};

class AStar
{
public:
  AStar(){};
  ~AStar();

  enum ASTAR_RETURN_STATE {
    SUCCESS = 1,
    ERR_MAXITERATION = 2,
    ERR_TIMEOUT = 3,
    ERR_OPENSETEMPTY = 4,
    ERR_INVALIDINPUT = 5,
    ERR_ONLYONEPOINT = 6
  };

  static const char *getError(const int error)
  {
    switch (error) {
    case SUCCESS:
      return "Success";
    case ERR_MAXITERATION:
      return "Max iteration reached";
    case ERR_TIMEOUT:
      return "0.2 seconds time limit exceeded";
    case ERR_OPENSETEMPTY:
      return "Open set is empty";
    case ERR_INVALIDINPUT:
      return "Invalid input";
    case ERR_ONLYONEPOINT:
      return "Only one point";
    default:
      return "Unknown error";
    }
  }

  void initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size);
  std::vector<Eigen::Vector3d> getPath();
  int AstarSearch(const Eigen::Vector3d &start_pt,
                  const Eigen::Vector3d &end_pt);

  typedef std::shared_ptr<AStar> Ptr;

private:
  GridMap::Ptr grid_map_;
  std::vector<std::shared_ptr<Node>> NodeMap_;
  std::vector<NodePtr> gridPath_;
  std::priority_queue<NodePtr, std::vector<NodePtr>, AstarComparator> openSet_;
  NodePtr startPtr_, endPtr_;

  Eigen::Vector3d orign_;     // origin of Astar map
  Eigen::Vector3d orign_map_; // origin of grid map
  Eigen::Vector3d dir_;
  Eigen::Vector3d step_;
  Eigen::Vector3i start_idx_, end_idx_;
  Eigen::Vector3i CENTER_IDX_, POOL_SIZE_;
  double reso_, inv_reso_;
  const double tie_breaker_ = 1.0 + 1.0 / 10000;

  int rounds_{0}; // Distinguish every call

  int toAddress(const Eigen::Vector3i &id);
  bool checkInput(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &end_pt);
  bool setMapCenter(const Eigen::Vector3d &center);
  bool Pos2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const;
  int checkOccupancy(const Eigen::Vector3d &pos);
  Eigen::Vector3d Index2Pos(const Eigen::Vector3i &index) const;

  double getDiagHeu(NodePtr node1, NodePtr node2);
  double getManhHeu(NodePtr node1, NodePtr node2);
  double getEuclHeu(NodePtr node1, NodePtr node2);
  double getHCost(NodePtr node1, NodePtr node2);
  double getGCost(int dx, int dy, int dz);

  int search(bool isGBFS, bool isSearchAll);
  bool expandPoint(Eigen::Vector3d pt,
                   Eigen::Vector3d step,
                   Eigen::Vector3i &idx);

  std::vector<NodePtr> retrievePath(NodePtr current);
};

/* -------------------------------------------------------------------------- */
/*                        definition of inline function                       */
/* -------------------------------------------------------------------------- */

inline bool AStar::setMapCenter(const Eigen::Vector3d &center)
{
  /**
   * Ensure that the distance between the origin and
   *  the origin of gridmap is an integer multiple of the grid spacing,
   *  which will make the Index2Pos/Pos2Index in Astar and
   *  Index2Pos/Pos2Index in gridmap return the same result
   */
  orign_ = center - CENTER_IDX_.cast<double>() * reso_;
  orign_ = orign_.cwiseMax(orign_map_);

  return grid_map_->roundWithOrigin(orign_);
}

inline int AStar::toAddress(const Eigen::Vector3i &id)
{
  return id(0) * POOL_SIZE_(1) * POOL_SIZE_(2) +
         id(1) * POOL_SIZE_(2) + id(2);
}

inline double AStar::getHCost(NodePtr node1, NodePtr node2)
{
  return tie_breaker_ * getDiagHeu(node1, node2);
}

inline Eigen::Vector3d AStar::Index2Pos(const Eigen::Vector3i &index) const
{
  Eigen::Vector3d pos;
  for (int i = 0; i < 3; ++i)
    pos(i) = (index(i) + 0.5) * reso_ + orign_(i);

  return pos;
}

inline bool AStar::Pos2Index(const Eigen::Vector3d &pt,
                             Eigen::Vector3i &idx) const
{
  for (int i = 0; i < 3; ++i)
    idx(i) = (int)std::floor((pt(i) - orign_(i)) * inv_reso_);

  if (idx(0) < 0 || idx(0) >= POOL_SIZE_(0) ||
      idx(1) < 0 || idx(1) >= POOL_SIZE_(1) ||
      idx(2) < 0 || idx(2) >= POOL_SIZE_(2))
  {
    cout << "pt: " << pt.transpose() << endl;
    cout << "idx: " << idx.transpose() << endl;
    cerr << "\033[1;31m[AStar]: index out of range!\033[0m" << endl;
    return false;
  }

  return true;
}

inline int AStar::checkOccupancy(const Eigen::Vector3d &pos)
{
  return grid_map_->getGridState(pos);
}

inline bool AStar::expandPoint(Eigen::Vector3d pt,
                               Eigen::Vector3d step,
                               Eigen::Vector3i &idx)
{
  // TODO 这里注意，现在就是默认除非有障碍物，否则都是可以走的
  // while (checkOccupancy(pt))
  while (checkOccupancy(pt) == 1)
  {
    pt = pt + step;
    if (!Pos2Index(pt, idx)) {
      cerr << "\033[1;31m[AStar]: expand point out of range!\033[0m" << endl;
      cerr << "pt: " << pt.transpose() << endl;
      return false;
    }
  }

  return true;
}

#endif
